Path planning system for autonomous off-road vehicles

ABSTRACT

A swath tracking system for an off-road vehicle includes a control system with a processor and a memory. The control system is configured to receive a plurality of vehicle location points and a current vehicle state, wherein the current vehicle state comprises a current vehicle location, generate a planned vehicle path through one or more of the plurality of vehicle location points, generate a correction path from the current vehicle location to a point along the planned vehicle path ahead of the current vehicle location along a direction of travel, generate a blended path by blending the planned vehicle path and the correction path based at least in part on an assigned weight, wherein the assigned weight is based at least in part on a heading error, a distance between the current vehicle location and the planned path, or a combination thereof, and guide the off-road vehicle along the blended path.

This application is a continuation of U.S. patent application Ser. No.15/627,074, now U.S. Pat. No. 10,188,021, entitled “PATH PLANNING SYSTEMFOR AUTONOMOUS OFF-ROAD VEHICLES,” filed Jun. 19, 2017, which is hereinincorporated by reference in its entirety.

BACKGROUND

The disclosure relates generally to off-road vehicle guidance systems,and more particularly to a path planning system for off-road vehicles.

Off-road vehicles, such as agricultural tractors, are typically manuallyguided through a field (e.g., via input from a driver). That is, anoperator controls the steering and speed of an off-road vehicle fromwithin a cab. Recent developments integrating GPS-based navigationsystems into agricultural vehicle control systems have enabled automaticor semi-automatic steering modes. For example, some agriculturalvehicles may include a control system configured to automatically directthe vehicle to follow a path between, over, or adjacent to rows in afield. The strip formed by the width of the vehicle and/or implement asthe vehicle follows the path may be referred to as a swath or pass.

In some cases, multiple off-road vehicles may be operating in the samegeographic area or field, such as an agricultural field. The off-roadvehicles may be towing agricultural implements of the same type or ofdifferent types, such as planters, harvesters, fertilizer applicationequipment, and so on. It may be useful to provide for improve pathplanning systems for one or more off-road vehicles.

BRIEF DESCRIPTION

Certain embodiments commensurate in scope with the originally claimedsubject matter are summarized below. These embodiments are not intendedto limit the scope of the claimed subject matter, but rather theseembodiments are intended only to provide a brief summary of possibleforms of the disclosure. Indeed, the disclosure may encompass a varietyof forms that may be similar to or different from the embodiments setforth below.

In one embodiment, a path planning system for off-road vehicles includesa processor. The processor is configured to receive, via a planrequestor and executor system, a plan request and to derive, via a swathand partition generator system, a set of partitions and a set of swathscorresponding to the set of partitions based on the plan request. Theprocessor is further configured to generate a partition hierarchy basedon the set of partitions and a map of a field, and to assign, via aswath connector, costs to potential connections between swaths in theset of swaths. The processor is additionally configured to derive afirst vehicle plan based on the costs to the potential connectionsbetween swaths, wherein the first vehicle plan comprises a first pathfor a first off-road vehicle to operate in the field.

In another embodiment, method of path planning for off-road vehiclesincludes receiving, via a plan requestor and executor system, a planrequest, and deriving, via a swath and partition generator system, a setof partitions and a set of swaths corresponding to the set of partitionsbased on the plan request. The method further includes generating apartition hierarchy based on the set of partitions and a map of a field,and assigning, via a swath connector, costs to potential connectionsbetween swaths in the set of swaths. The method additionally includesderiving a first vehicle plan based on the costs to the potentialconnections between swaths, wherein the first vehicle plan comprises afirst path for a first off-road vehicle to operate in the field.

In a further embodiment, a tangible, non-transitory computer readablemedium includes instructions configured to receive, via a plan requestorand executor system, a plan request and to derive, via a swath andpartition generator system, a set of partitions and a set of swathscorresponding to the set of partitions based on the plan request. Theinstructions are further configured to generate a partition hierarchybased on the set of partitions and a map of a field, and to assign, viaa swath connector, costs to potential connections between swaths in theset of swaths. The instructions are additionally configured to derive afirst vehicle plan based on the costs to the potential connectionsbetween swaths, wherein the first vehicle plan comprises a first pathfor a first off-road vehicle to operate in the field.

DRAWINGS

These and other features, aspects, and advantages of the presentdisclosure will become better understood when the following detaileddescription is read with reference to the accompanying drawings in whichlike characters represent like parts throughout the drawings, wherein:

FIG. 1 is a schematic diagram of an embodiment of off-road vehicles andagricultural implements within an agricultural field;

FIG. 2 is a schematic diagram of an embodiment of a control systems thatmay be utilized to control the off-road vehicles of FIG. 1;

FIG. 3 is bock diagram illustrating an embodiment of a modular pathplanning system for planning operations of the off-road vehicles of FIG.1;

FIG. 4 is a top-down view of an embodiment of a segment of a plannedoff-road vehicle path that may be planned via the modular path planningsystem of FIG. 3;

FIG. 5 is the figure illustrates an embodiment of a field that may havebeen used as input into the modular path planning system of FIG. 3 andsubsequently partitioned; and

FIG. 6 is a flow chart of an embodiment of process for deriving vehicleplan via the modular path planning system of FIG. 3.

DETAILED DESCRIPTION

The techniques described herein provide for an autonomous vehicle,modular path planning system. In one embodiment, the path planningsystem includes a modular system architecture, as further describedbelow, with components that may be communicating, for example, inreal-time. The modular path planning system may derive a plan thatenables one or more off-road vehicles to more efficiently operate on afield, such as an agricultural field. In certain embodiments, amulti-stage approach may executed via the modular path planning systemto generate a plan to accomplish desired agricultural operations withgiven resources, e.g., off-road vehicle(s), agricultural implements,agricultural supplies (e.g., seed, fertilizer, etc.), field geography,and so on. The plan may then be updated, for example during operationsof the off-road vehicle(s), to account for changes such as changed fieldconditions, weather, and the like, and an updated plan executed. In thismanner, one or more off-road vehicles may be operated more efficiently,improving agricultural operations.

Turning now to FIG. 1, the figure is a schematic diagram of anembodiment of a first off-road vehicle 10 and an agricultural implement12, and a second off-road vehicle 14 and an agricultural implement 16within an agricultural field 18. The off-road vehicles 10 and 14 (e.g.,tractor or other prime mover) are configured to tow the agriculturalimplements 12 and 16 throughout the field 18 along a direction of travel20 and 22, respectively. In certain embodiments, the off-road vehicles10, 14 are directed (e.g., via an automated system) along a path 24 anda path 26 to traverse the field 18 along substantially parallel rows 28.Path 24 starts at location 30 and ends at location 32, while path 26starts at location 34 and ends at location 36. While two off-roadvehicles 10, 14 are shown, it is to be understood that, in certainexamples, a single off-road vehicle 10 or 12 may be used, and in otherexamples, three or more off-road vehicles 10 and/or 12 may be used.

As the vehicles 10, 14 tow the implements 12, 16 through the field 18along paths 24, 26, the width of the implements 12, 16 each covers arespective swath through the field 18, as described in further detailbelow. In alternative embodiments, the off-road vehicles 10, 14 may bedirected to traverse the field 18 along other routes (e.g., along aspiral path, etc.). As will be appreciated, the agricultural implements12, 16 may be any suitable implement for performing agriculturaloperations throughout the field 18. For example, in certain embodiments,the agricultural implements 12, 16 may be a tillage tool, a fertilizerapplication tool, a seeding or planting tool, or a harvesting tool,among others. While the agricultural implements 12, 16 are towed by theoff-road vehicle 10, 14 in the illustrated embodiment, it should beappreciated that in alternative embodiments, the agricultural implements12, 16 may be integrated within the off-road vehicles 10, 14.

In certain embodiments, the off-road vehicles 10, 14 are configured tooperate autonomously (e.g., without an operator present in a cab of theoff-road vehicle 10, 14). Accordingly, an automatic system may directthe off-road vehicles 10 towing agricultural implement 12 and theoff-road vehicle 14 towing agricultural implement 16 along the paths 24,26 throughout the field 18 without direct control by an operator. Insuch embodiments, the autonomous off-road vehicles 10 and/or 14 may beconfigured to communicate with a base station 38. For example, in someembodiments, the off-road vehicles 10, 14 may receive plan data from thebase station 38. The plan data may partition the field 18 into one ormore partitions as described in more detail below, and may assign theoff-road vehicles 10, 14 to the various partitions. Likewise, the plandata may derive certain swaths for each of the off-road vehicles 10, 14to follow.

Furthermore, in certain embodiments, the off-road vehicles 10, 14 may beconfigured to operate semi-autonomously. For example, an operator withinthe cab may direct the off-road vehicle 10 and/or 14 to startingpositions 30, 34 within or proximate to the field 18, and an automatedsystem may direct the off-road vehicle 10 and/or 14 along theselected/planned route (e.g., the path 24, 26) through the field 18while operator(s) monitors operation of the off-road vehicles 10, 14and/or the implements 12, 16.

During operation, the off-road vehicle 10 may encounter changesconditions, such as overflowing ponds, muddy areas, and so on, and mayalso diverge from the planned paths 24, 26 or swath. This divergence maybe due to operator inputs, geographical features in the field 18, anobstacle in the field 18, changes in the off-road vehicles' 10, 14operation due to various field conditions (mud, slippery ground, largeclods of dirt, etc.), and the like. An updated plan may be generated toaccount for the various changes in the field 18, field operations,operator inputs, and, so on.

FIG. 2 is a schematic diagram of an embodiment of an automated fieldoperations system 100 that may be utilized to control the off-roadvehicles 10 and/or 14 of FIG. 1. In the illustrated embodiment, theautomated field operations system 100 includes a primary control system102 (e.g., mounted on the off-road vehicles 10, 14), and the off-roadvehicles 10, 14 includes a first transceiver 104 configured to establisha wireless communication link with a second transceiver 106 of the basestation 38. As will be appreciated, the first and second transceivers102, 104 may operate at any suitable frequency range within theelectromagnetic spectrum. For example, in certain embodiments, thetransceivers 102, 104 may broadcast and receive radio waves within afrequency range of about 1 GHz to about 10 GHz. In addition, the firstand second transceivers 102, 104 may utilize any suitable communicationprotocol, such as a standard protocol (e.g., Wi-Fi, Bluetooth, etc.) ora proprietary protocol.

In the illustrated embodiment, the off-road vehicles 10, 14 includes aspatial locating device 108, which is mounted to the off-road vehicles10, 14 and configured to determine positions of the off-road vehicles10, 14. As will be appreciated, the spatial locating device 108 mayinclude any suitable system configured to determine the positions of theoff-road vehicles 10, 14, such as a global positioning system (GPS) orGlobal Navigation Satellite System (GLONASS), for example. The spatiallocating device 108 is in communication with the primary vehicle controlsystem 102. The spatial locating device 108 may include a GlobalNavigation Satellite System (GNSS) receiver 110 configured tocommunicate with two or more satellites in orbit (e.g., GPS, GLONASS,Galileo, BeiDou, etc.) to determine the location, heading, speed, etc.of the vehicles 10, 14. The receiver 110 may include one or moremicro-processors 112, a memory component 114, an input/output system116, a power supply 118, and radio circuitry 120. The processors 112 mayrun software stored on the memory component 114 to compute the positionsof the vehicle 10, 14. Based on the computed position over time, theprocessor 112 may also determine vehicle 10, 14 heading, speed, etc. Insome embodiments, the spatial locating device 108 may include athree-dimensional imaging system, such as a Lidar, which may be used inconjunction with the GNSS and configured to assist in determining adesired path and determining the vehicle's position and orientationrelative to the path. Additionally, the spatial locating device 108 mayinclude an augmentation system, such as Real Time Kinematic (RTK) orWide Area Augmentation System (WAAS) to improve the accuracy of thespatial locating device 108.

In certain embodiments, the spatial locating device 108 may beconfigured to determine the positions of the off-road vehicle 10, 14relative to a fixed point within the field (e.g., via a fixed radiotransceiver 106). Accordingly, the spatial locating device 108 may beconfigured to determine the positions of the off-road vehicle 10, 14relative to a fixed global coordinate system (e.g., via the GPS) or afixed local coordinate system. In certain embodiments, the firsttransceiver 104 is configured to broadcast a signal indicative of theposition of the off-road vehicles 10, 14 to the transceiver 106 of thebase station 38. Based on the determined locations of the off-roadvehicle 10, 14, the control system 102 may determine (e.g., via aprocessor) the relative proximity to one or more rows or swaths, one ormore boundaries, one or more headlands, etc. Furthermore, the controlsystem 102 may determine a planned path through points or curvesreceived from the base station 38, and/or a correction path from thevehicle's current position back to the planned paths 24, 26.

In the illustrated embodiment, the off-road vehicles 10, 14 include asteering control system 122 configured to control direction of movementof the off-road vehicles 10, 14 and a speed control system 124configured to control a speed of the off-road vehicles 10, 14.Furthermore, the control system 102 includes a controller 126communicatively coupled to the first transceiver 104, to the spatiallocating device 108, to the steering control system 122, and to thespeed control system 124. In certain embodiments, the controller 126 maycontrol the steering control system 122 and the speed control system 124to guide the off-road vehicles 10, 14 along the planned path.

In further embodiments, the controller 126 is configured to control thesteering control system 122 and the speed control system 124 to guidethe off-road vehicles 10, 14 based on a path or paths planned via amodular path planning system. A correction path controller 128 isconfigured to receive the current vehicle state (e.g., position,velocity, heading, curvature, etc.), the planned path, and an estimateddelay time of the control loop (e.g., to estimate the state of thevehicle when the vehicle actuates changes in the control signals). Thecorrection path controller 128 may operate continuously to determinewhether the vehicles 10, 14 are off the planned path. If so, thecorrection path controller 128 determines an error (e.g., a distancebetween the planned path and the current vehicle 10, 14 position or aheading error) and generates a continuous path for the vehicle 10, 14 tofollow to a point along the planned path.

The correction path controller 128 includes a processor, such as theillustrated microprocessor 130, and a memory device 132. The correctionpath controller 128 may also include one or more storage devices and/orother suitable components. The processor 130 may be used to executesoftware, such as software for generating a correction path, and soforth. The processor 130 may include multiple microprocessors, one ormore “general-purpose” microprocessors, one or more special-purposemicroprocessors, and/or one or more application specific integratedcircuits (ASICS), or some combination thereof. For example, theprocessor 130 may include one or more reduced instruction set (RISC)processors. The memory device 132 may include a volatile memory, such asrandom access memory (RAM), and/or a nonvolatile memory, such as ROM.The memory device 132 may store a variety of information and may be usedfor various purposes. For example, the memory device 132 may storeprocessor-executable instructions (e.g., firmware or software) for theprocessor 130 to execute, such as instructions for generating acorrection path. The storage device(s) (e.g., nonvolatile storage) mayinclude ROM, flash memory, a hard drive, or any other suitable optical,magnetic, or solid-state storage medium, or a combination thereof. Thestorage device(s) may store data (e.g., target points, the planned path,etc.), instructions (e.g., software or firmware for controlling theoff-road vehicle, etc.), and any other suitable data. In someembodiments, the tasks of the correction path controller 128 and theprimary controller 126 may be performed by a single controller (e.g.,the primary controller 126).

In certain embodiments, the controller 126 is an electronic controllerhaving electrical circuitry configured to process data from thetransceiver 104, the spatial locating device 108, the correction pathcontroller 128, or a combination thereof, among other components of theoff-road vehicles 10, 14. In the illustrated embodiment, the controller126 includes a processor, such as the illustrated microprocessor 134,and a memory device 136. The controller 126 may also include one or morestorage devices and/or other suitable components. The processor 134 maybe used to execute software, such as software for controlling theoff-road vehicles 10, 14 and so forth. Moreover, the processor 134 mayinclude multiple microprocessors, one or more “general-purpose”microprocessors, one or more special-purpose microprocessors, and/or oneor more application specific integrated circuits (ASICS), or somecombination thereof. For example, the processor 134 may include one ormore reduced instruction set (RISC) processors.

The memory device 136 may include a volatile memory, such as randomaccess memory (RAM), and/or a nonvolatile memory, such as ROM. Thememory device 136 may store a variety of information and may be used forvarious purposes. For example, the memory device 136 may storeprocessor-executable instructions (e.g., firmware or software) for theprocessor 134 to execute, such as instructions for controlling theoff-road vehicles 10, 14. The storage device(s) 136 (e.g., nonvolatilestorage) may include read-only memory (ROM), flash memory, a hard drive,or any other suitable optical, magnetic, or solid-state storage medium,or a combination thereof. The storage device(s) 136 may store data(e.g., target points, vehicle paths, etc.), instructions (e.g., softwareor firmware for controlling the off-road vehicle, etc.), and any othersuitable data.

In the illustrated embodiment, the steering control system 122 includesa wheel angle control system 138, a differential braking system 140, anda torque vectoring system 142. The wheel angle control system 138 mayautomatically rotate one or more wheels or tracks of the off-roadvehicles 10, 14 (e.g., via hydraulic actuators) to steer the off-roadvehicles 10, 14 along the determined path through the field 18. By wayof example, the wheel angle control system 138 may rotate frontwheels/tracks, rear wheels/tracks, and/or intermediate wheels/tracks ofthe off-road vehicles 10, 14, either individually or in groups. Thedifferential braking system 140 may independently vary the braking forceon each lateral side of the off-road vehicles 10, 14 to direct theoff-road vehicles 10, 14 along the determined paths through the field.Similarly, the torque vectoring 142 may differentially apply torque fromthe engine to wheels and/or tracks on each lateral side of the off-roadvehicles 10, 14, thereby directing the off-road vehicle 10, 14 along thedetermined path through the field. While the illustrated steeringcontrol system 122 includes the wheel angle control system 138, thedifferential braking system 140, and the torque vectoring system 142, itshould be appreciated that alternative embodiments may include one ortwo of these systems, in any suitable combination. Further embodimentsmay include a steering control system having other and/or additionalsystems to facilitate directing the off-road vehicles 10, 14 along thedetermined path through the field (e.g., an articulated steering system,etc.).

In the illustrated embodiment, the speed control system 124 includes anengine output control system 144, a transmission control system 146, anda braking control system 148. The engine output control system 144 isconfigured to vary the output of the engine to control the speed of theoff-road vehicles 10, 14. For example, the engine output control system144 may vary a throttle setting of the engine, a fuel/air mixture of theengine, a timing of the engine, or a combination thereof, and/or othersuitable engine parameters to control engine output. In addition, thetransmission control system 146 may adjust gear selection within atransmission to control the speed of the off-road vehicles 10, 14.Furthermore, the braking control system 148 may adjust braking force,thereby controlling the speed of the off-road vehicles 10, 14. While theillustrated speed control system 124 includes the engine output controlsystem 144, the transmission control system 146, and the braking controlsystem 148, it should be appreciated that alternative embodiments mayinclude one or two of these systems, in any suitable combination.Further embodiments may include a speed control system 124 having otherand/or additional systems to facilitate adjusting the speed of theoff-road vehicle.

As previously discussed, the off-road vehicles 10, 14 are configured tocommunicate with the base station 38 via the transceivers 104, 106. Inthe illustrated embodiment, the base station 38 includes a controller150 communicatively coupled to the base station transceiver 106. Thecontroller 150 is configured to output commands and/or data to theoff-road vehicles 10, 14. For example, the controller 150 may provide aseries of points or curves to the off-road vehicle 10, 14 via thetransceivers 104, 106, from which the vehicles 10. 14 generate plannedpaths.

In certain embodiments, the controller 150 is an electronic controllerhaving electrical circuitry configured to process data from certaincomponents of the base station 38 (e.g., the transceiver 106). In theillustrated embodiment, the controller 150 includes a processor, such asthe illustrated microprocessor 152, and a memory device 154. Theprocessor 152 may be used to execute software, such as software forproviding commands and/or data to the off-road vehicle controller 126,and so forth. Moreover, the processor 152 may include multiplemicroprocessors, one or more “general-purpose” microprocessors, one ormore special-purpose microprocessors, and/or one or more applicationspecific integrated circuits (ASICS), or some combination thereof. Forexample, the processor 152 may include one or more reduced instructionset (RISC) processors. The memory device 154 may include a volatilememory, such as random access memory (RAM), and/or a nonvolatile memory,such as read-only memory (ROM). The memory device 154 may store avariety of information and may be used for various purposes. Forexample, the memory device 154 may store processor-executableinstructions (e.g., firmware or software) for the processor 152 toexecute, such as instructions for providing commands and/or data to theoff-road vehicle controller 126.

In the illustrated embodiment, the base station 38 includes a userinterface 156 communicatively coupled to the controller 150. The userinterface 156 is configured to present data from the off-road vehicles10, 14 and/or the agricultural implements 12, 16 to an operator (e.g.,data associated with operation of the off-road vehicles, data associatedwith operation of the agricultural implements, etc.). The user interface156 is also configured to enable an operator to control certainfunctions of the off-road vehicles 10, 14 (e.g., starting and stoppingthe off-road vehicles, instructing the off-road vehicles to follow aselected/planned route through the field, etc.). In the illustratedembodiment, the user interface includes a display 158 configured topresent information to the operator, such as the position of theoff-road vehicles 10, 14 within the field, the speed of the off-roadvehicle, and the path of the off-road vehicle, among other data. Inaddition, the user interface 156 (e.g., via the display 158, via anaudio system, etc.) is configured to alert an operator if the vehicle 10and/or 14 substantially diverges from the planned path.

In the illustrated embodiment, the base station 38 includes a storagedevice 160 communicatively coupled to the controller 150. The storagedevice 160 (e.g., nonvolatile storage) may include read-only memory(ROM), flash memory, a hard drive, or any other suitable optical,magnetic, or solid-state storage medium, or a combination thereof. Thestorage devices 160 may store data (e.g., field maps, planned paths orvehicle routes, etc.), instructions (e.g., software or firmware forcommanding the off-road vehicle, etc.), and any other suitable data.

While the off-road vehicle control system 102 includes the off-roadvehicle controller 126 and the correction path controller 128, it shouldbe appreciated that in alternative embodiments, the control system 102may include the base station controller 150. For example, in certainembodiments, control functions of the control system 102 may bedistributed between the off-road vehicle controller 126, the correctionpath controller 128, and the base station controller 150. In furtherembodiments, the base station controller 150 may perform a substantialportion of the control functions of the control system 102. For example,in certain embodiments, the transceiver 104 may output signals to thetransceiver 106. In such embodiments, the transceiver 104 may outputcorresponding signals to the base station controller 150, and the basestation controller 150 may determine the appropriate action based on thecurrent state of the off-road vehicles 10, 14 and output a signalindicative of the appropriate action. For example, the controller 150may output a signal to the user interface 156, instructing the userinterface 156 to alert the operator of the off-road vehicle's divergencefrom the planned path. In addition, the controller 150 may outputinstructions to the off-road vehicle controller 126 (e.g., via thetransceivers 104 and 106), instructing the off-road vehicles 10, 14and/or the agricultural implements 12, 16 to perform certain operations(e.g., steering control and/or speed control) to guide the off-roadvehicles 10, 14 along the planned path or to guide the off-road vehicles10, 14 back to the planned path.

Also shown are a modular path planning system 162, which will bedescribed if further detail below. The modular path planning system 162or certain systems of the modular path planning system 162 may bedisposed in the vehicles 10, 14, in the base station 38, or in acombination thereof. The modular path planning system 162 may be used toderive a plan that may include for example, swaths and partitions, swathconnections, point-to-point plans, and so on, suitable for enabling oneor more vehicles, such as the off-road vehicles 10, 14, tosimultaneously operate in the field 18 in a more efficient manner.

FIG. 3 is bock diagram illustrating an embodiment of the modular pathplanning system 162. In the illustrated embodiment, the modular pathplanning system 162 includes various subsystems, more specifically, aplan requestor and executor system 164, a swath and partition generatorsystem 166, a choreographer system 168, a point-to-point planner system172, and a traveling salesman problem (TSP) solver system 174. Thesystem 162 and subsystems 164, 166, 168, 170, 172, and/or 174 may beimplemented as computer code or instructions stored in the devices 132,136, 154, 160 and executable via the processors 130, 134, 152. Thesubsystems 164, 166, 168, 170, 172, and/or 174 may execute concurrentlyor sequentially with respect to each other, and may communicate witheach other with a variety of techniques, including interprocesscommunication (IPC) techniques, shared memory, message passing, socketcommunications, message ques, mapped files, and so on.

In operations, the modular path planning system 162 may interface withvarious external systems 176, such as controllers (e.g., controllers126, 128, 150), user interfaces (e.g., user interface 156), and othersystems (e.g., weather forecasting systems, dispatch systems). Forexample, the operator of the off-road vehicles 10, 14 may enter certaininformation (e.g., as part of a plan request 178) into the planrequestor and executor system 164, such as type of off-road vehicles tobe used (e.g., tractor, combine, and so on), manufacturer, vehiclemodel, vehicle year, vehicle effective width, vehicle turning radius,type of agricultural implements 12, 16 to be used (e.g., planter,fertilizer, seeder, etc.), implement manufacturer, implement year,implement effective width, implement turning radius, and the like.

Other inputs that may be part of the plan request 178 may include a mapof the field 18, desired area(s) to cover in the field 18, obstacleboundaries, field 18 boundaries, and so on. Operation informationincluded in the plan request 178 may include the type of operation(s) tobe performed (e.g., planting, seeding, tilling, fertilizing, etc.) anddetails of the operation (e.g., type of seed, type of fertilizer, dateand time of day for operations, etc.). Accordingly, the plan request 178may be transmitted, detailing the aforementioned information.

The modular path planning system 162 may use the plan request 178 asinput to derive one or more vehicle plans 180 via the systems 164, 166,168, 170, 172, and/or 174, as further described below. The vehicleplan(s) 180 may include inter-vehicle coordination suitable for enablingmultiple vehicles to operate in the same field, e.g., field 18. Thevehicles 10, 14 may then use the vehicle plan(s) 180 to drive andoperate in the field 18. For example, the vehicle plan(s) 180 mayinclude partitions, swaths, paths, path segments, and/or driving pointsto be observed by the vehicles 10, 14. Changes, such as a vehicle 10, 14undergoing undesired maintenance, weather conditions, obstacle changes(e.g., pond overflowing), and the like, may result in updated plan(s)182. The updated plan(s) 182 may include a simple reassignment of aswath to another vehicle, may include new swaths to follow, may includeevasive actions to take, or may include a complete new vehicle plan.Accordingly, the vehicles 10, 14 may be operated with up-to-dateinformation and may thus cover the field 18 in a more efficient manner.

In order to derive the vehicle plans 180, 182, the modular path planningsystem 162 may use the plan request 178 to communicate with the swathand partition generator system 166 and the swath connector system 170.The swath partition generator system 166 may apply the plan request 178to divide the field 18 into one or more partitions and may establishrelationships between the partitions, (e.g., Partition A boundsPartition B, Partition B bounds partitions C and D, partition C boundspartition D, Partition D does not bound any partition). In oneembodiment, the partitions and their bounding relationships form adirected graph where all the edges are directed from one vertex toanother (meaning partition bounds cannot form a loop).

Partitions are comprised of swaths, where a single vehicle/implement maybe assigned to a single swath. A swath includes a width at least as wideas the width of the vehicle and/or agricultural implement that will beassigned the swath. The swath and partition generator system 166receives the information contained in the plan request 178 and createswell-defined swaths and headlands in the field 18. A continuouscurvature path is called a swath, and may consist of line segments,arcs, or clothoids. Groups of similar swaths are called swathpartitions. The created swaths may include headland/cleanup paths,interior rows, or obstacle cleanup paths and may be grouped intopartitions. Swaths are typically assigned to one vehicle, or assigned toseveral candidate vehicles (for example, when several vehicles have thesame coverage width). A partition hierarchy is also generated, whichdescribes how the partitions are related relative to anoutward-to-inward spatial relationship; that is, a directed graph thatrepresents to some degree ordering of operations.

Each outer and obstacle boundary in the field 18 has 0 or moreheadland/cleanup-laps associated with it and the swaths in each lapcomprise a single partition. Swaths generated in the interior of thefield 18 make up the remaining partitions. For each headland/clean-uppartition all the swaths are assigned to a single vehicle, or a list ofcandidate vehicles. For inner partitions the swaths are staggered on aper vehicle basis. Once the partitions and their associated swaths arecreated, the information from the original plan request 178 and thepartitions are sent to the swath connector system 170.

The swath connector system 170 may receive the plan request 178 andderived swath and partitions 184 and then examines cost constraints inthe plan request information 178. Cost constraints may include fuelcosts, cost of navigating muddy ground versus rocky ground, seed costs,fertilizer costs, equipment wear and tear costs, travel to startingareas, and so on. With this information, the operations desired dictatea relative ordering (outward-to-inward or inward-to-outward)relationship between the partitions so as to optimize operations.

Once the order of partitions 184 is established, potential swathconnections are evaluated; that is, the paths between swath endpoints. Apotential swath connection is a path from the end of one swath to an endof another swath such that the vehicle/implement does not collide withany obstacles. Each potential swath connection is assigned a cost basedon a weighted average of several subcosts. The subcosts may include timeof travel along the path, length of path, length of path not on a road,acceleration/deceleration requirements for travel along the path, amountof trim-back on swath, and so on.

The swath connector system 170 may then formulate the swath connectorproblem as a traveling salesperson (TSP) problem, where each “city” inthe TSP is represented as an end-point of the swath. The purpose of theTSP is to connect the end-points of each swath in the lowest-costordering, per the costs listed above. Connections 186 between the swathend-points are made by use of calls to the point-to-point planner system172, which generates continuous curvature paths (clothoids) 188 betweentwo points. More specifically, point-to-point planner system 172receives a map, vehicle information, a start pose (position andorientation and curvature) and an end pose (position and orientation adcurvature), and creates a continuous curvature plan between the twoposes considering the vehicle 10, 14, information (turn radius, speed,vehicle size) and map (drivable areas, obstacles, and so on).

Once the swath connection problem is formulated as a TSP problem, theproblem is then passed to the TSP solver system 174. The TSP solversystem 174 may apply a variety of TSP techniques such as Christofides'algorithm, nearest neighbor algorithm, bitonic tour, branch-and-boundalgorithms, and so on, to solve the TSP problem either exactly orapproximately. Once the TSP solver system 174 returns a solution (e.g.,a path from start city to end city), the solution is checked forvalidity, for example to ensure that all “cities” are presently visited.If more optimization or updated costs are to be calculated, the TSPsolver system 174 may be called repeatedly with different initialconditions or costs, until satisfaction criteria are met, thus findingan optimal or near-optimal solution.

The choreographer system 168 may “choreograph” the vehicles 10, 14 toavoid vehicle 10, 14 and/or agricultural implement 12, 16 collisions atpath intersections, when traveling in opposite directions on adjacentswaths, and/or when traveling in the same direction over-taking oneanother. For example, the choreographer system 168 may increase cost,such as a path(s) cost, as the system 168 derives that one or more pathsmay lead to a collision. The raised cost may then be used to recalculateor otherwise change the path(s) so that collisions may not occur. Thechoreographer system 168 may be called during a plan execution by theplan requestor and executor system 164 to identify the above problems.The choreographer system 168 may also be called by other subsystems,such as the swath connector 170, to help anticipate any problems presentin the current plan, and provide solutions to mitigate any problematicbehavior.

Upon the desired level of optimality, the swath connector system 170returns the path plan 180 for each vehicle 10, 14. The path plan 180 isdelivered to the plan requestor and executor system 164, which may thenprovide the path plan 180 to external systems 176. Likewise, changes tothe plan 180 may be similarly computed via the systems 164, 166, 168,170, 172, 174, resulting in the updated plan 182.

It may be beneficial to illustrate certain path segments, such as anexample segment 200 illustrated in FIG. 4, and partitions illustrated inFIG. 5. More specifically, FIG. 4 is a top-down view of a segment 200 ofa planned off-road vehicle path 180 and/or 182. In the illustratedembodiment, the path segment 200 is a path through an end-of-row turnthat connects the end of one row to the beginning of another row, whileavoiding contact with a boundary 202 (e.g., a fence).

In the present embodiment, the path segment 200 includes five discreteclothoid segments 204, 206, 208, 210, 212 that combine to form acontinuous path from a starting point 214 to an end point 216. Clothoidsegments, also referred to as Euler spirals, are defined as curves witha curvature that changes linearly along the curve length. Put anotherway, the rate of curvature σ for a given clothoid is constant. Clothoidsare C2 continuous and may take into account physical constraints, suchas minimum turning radius and maximum turning rate of the vehicle. Aclothoid may be defined as a curve having a curvature that varieslinearly with curve length. The velocity ({dot over (x)}(s), {dot over(y)}(s)) at a distance s along the clothoid, may be described by thefollowing equations:

$\begin{matrix}{{{\overset{.}{x}(s)} = {\cos\left( {{\frac{\sigma}{2}s^{2}} + {\kappa_{i}s} + q_{i}} \right)}},} & (1) \\{{{\overset{.}{y}(s)} = {\sin\left( {{\frac{\sigma}{2}s^{2}} + {\kappa_{i}s} + q_{i}} \right)}},} & (2)\end{matrix}$where q_(i) is the initial heading, κ_(i) is the initial curvature, andσ is the rate of change of the curvature with respect to s, the distancealong the clothoid. The curvature as a function of s may be describedby:κ=σs+κ _(i),  (3)

The heading as a function of s may be described by:

$\begin{matrix}{q = {{\frac{\sigma}{2}s^{2}} + {\kappa_{i}s} + {q_{i}.}}} & (4)\end{matrix}$

Thus, the position (x(s), y(s)) may be described by:

$\begin{matrix}{{{x(s)} = {x_{i} + {\int_{0}^{s}{{\cos\left( {{\frac{\sigma}{2}\tau^{2}} + {\kappa_{i}\tau} + q_{i}} \right)}d\;\tau}}}},{and}} & (5) \\{{{y(s)} = {y_{i} + {\int_{0}^{s}{{\sin\left( {{\frac{\sigma}{2}\tau^{2}} + {\kappa_{i}\tau} + q_{i}} \right)}d\;\tau}}}},} & (6)\end{matrix}$where τ is path length. Thus, a clothoid segment may be based on initialposition, x_(i) and y_(i), initial heading, q_(i), initial curvature,κ_(i), rate of curvature, σ, and path length, s. The capabilities of thevehicles 10, 14 and/or agricultural implements 12, 16 (e.g., minimumturning radius, maximum turning rate) may be taken into account whendetermining the parameters of the clothoid segments 204, 206, 208, 210,212. For example, the maximum turning rate (e.g., maximum slew rate) ofthe vehicle may be proportional to a maximum value for rate of curvatureσ. The minimum turning radius may determine a maximum value forcurvature κ. It should be appreciated that segments having a constantcurvature are clothoids with a rate of curvature σ of zero. Similarly,straight line segments are clothoid segments in which both the curvatureκ and the rate of curvature σ are zero. Thus, straight line segments,segments of constant curvature, and segments having a constant rate ofcurvature may all be defined as clothoid segments. Accordingly, the pathsegment 200 of five clothoid segments 204, 206, 208, 210, 212 may beparameterized in a single vector:z=[x ₀ ,y ₀ ,q ₀,κ₀,σ₀ ,s ₀ ,x ₁ ,y ₁ ,q ₁,κ₁,σ₁ ,s ₁ , . . . ,x ₄ ,y ₄,q ₄,κ₄,σ₄ ,s ₄]^(T)  (7)

The off-road vehicle control system 102, the base station controller150, and/or the modular path planning system 162 may adjust one or moreof the initial position x; y; the initial heading q; the initialcurvature κ the rate of curvature σ, and the path length s of eachsegment 204, 206, 208, 210, 212, such that the path segment 200 is C2continuous. The path segment 200 is C2 continuous when the finalposition, the final heading, and the final curvature at the end of onesegment (e.g., the first segment 204) matches the initial position, theinitial heading, and the initial curvature at the start of theproceeding segment (e.g., the second segment 206).

If the vehicle 10 or 14 is at the starting point 214, then the currentvehicle state may be used as the initial position, initial heading, andinitial curvature of the first clothoid segment 204. Conversely, theparameters of the last segment 212 may be adjusted such that the finalposition, final heading, and final curvature of the last segment 212match the final position, final heading, and final curvature of thedesired exit state. As previously discussed, the parameters of eachsegment 204, 206, 208, 210, 212 are adjusted in such a way that the rateof curvature σ does not exceed the vehicle's maximum turning rate,and/or the curvature κ does not fall below the minimum turning radius ofthe vehicle. Additionally, a minimum segment length (e.g., 10centimeters) may be used such that the path lengths of each segment 204,206, 208, 210, 212 remains above the minimum segment length.

The path segment 200 may also be configured to avoid boundaries 202 whengenerating clothoid segments 204, 206, 208, 210, 212 of the path segment200 and adjusting the parameters of the segments. In some applications,the implement 12, 16 may be wider than the vehicles 10, 14. In suchembodiments, the path segment 200 may include a buffer region 218 oneither side of the path segment 200 that may be as wide as or wider thanthe implements 12 or 16, such that the implements 12 or 16 may becapable of traversing the path segment 200 without coming into contactwith the boundary 202.

Turning now to FIG. 5, the figure illustrates an embodiment of a field220 that may have been used as input as part of the information includedin the plan request 178 and subsequently partitioned. The modular pathplanning system 162 may then partition the field 220 into one or morepartitions, among other derived features. In the illustrated embodiment,the field 220 includes an L-shape. The modular path planning system 162may derive that the field 220 would be more efficiently operated on byrespecting multiple partitions. For example, an exterior boundary 222may be derived. The exterior boundary 222 may define outer bounds of thefield 220. Headland partitions 224, 226 may then be derived, suitablefor turning around the field 220. Interior partition 228 are alsoderived, having various rows 230. Inside of the interior partition 228 aheadlands obstacle partition 232 is derived, which surrounds an obstacle234, such as a pond. A second interior partition 236 is also derived,having rows 238 perpendicular to rows 230.

As mentioned previously, an order of operations for the partitions shownmay also be derived. For example, a cost function may determine that itmay be more efficient, when planting, to plant inside of partitions 228first, followed by a planting inside partition 236 second. Accordingly,the vehicle plan 180, 182 may follow certain headlands and roworientations in a desired planting order. When harvesting, the order maybe different, thus resulting in a harvesting order. Indeed, differentoperations (e.g., planting, seeding, fertilizing, harvesting), differentimplements 12, 16, different implement settings, different vehicle 10,14 types, and so on, may result in different order of partitionoperations. By deriving multiple partitions, partition types, partitionorder of operations, swaths, and so on, the techniques described hereinmay improve planning operations, resulting in a more efficient andproductive vehicle plan(s) 180, 182.

FIG. 6 is a flow chart of an embodiment of a process 300 suitable forderiving the vehicle plan(s) 180 or plan updates 182. The process 300may be implemented as computer code or instructions, for example,included in the modular path planning system 162 and executable via theprocessors 150 and/or 102. In the depicted embodiment, the process 300may receive (block 302) a plan request, such as the request 178. Asmentioned previously, the plan request 178 may include the type ofoff-road vehicles 10, 14 to be used (e.g., tractor, combine, and so on),manufacturer, vehicle model, vehicle year, vehicle effective width,vehicle turning radius, type of agricultural implements 12, 16 to beused (e.g., planter, fertilizer, seeder, etc.), implement manufacturer,implement year, implement effective width, implement turning radius, andthe like.

The plan request 178 may additionally include a map of the field 18,desired area(s) to cover in the field 18, obstacle boundaries, field 18boundaries, and so on. Operation information included in the planrequest 178 may include the type of operation(s) to be performed (e.g.,planting, seeding, tilling, fertilizing, etc.) and details of theoperation (e.g., type of seed, type of fertilizer, date and time of dayfor operations, etc.). The process 300 may then derive (block 304) a setof swath and partitions. For example, the process 300 may execute theswath and partition generator system 166 to generate one or morepartitions and one or more swaths to follow. The partitions may beordered (block 306), for example hierarchically. For example, The swathpartition generator system 166 may apply the plan request 178 to dividethe field 18 into one or more partitions and may establish relationshipsbetween the partitions, such as bounding relationships (e.g., PartitionA bounds Partition B, Partition B bounds partitions C and D, partition Cbounds partition D, Partition D does not bound any partition).Accordingly, the partitions may be ordered to form a directed graphwhere all the edges are directed from one vertex to another (meaningpartition bounds cannot form a loop). The edge between two vertices maybe a bounding relationship, between two partitions. For the example ofPartition A bounds Partition B, Partition B bounds partitions C and D,partition C bounds partition D, Partition D does not bound anypartition, the edges may be A→B, B→C, B→D, C→D.

The process 300 may then assign costs (block 308) to potential swathconnections. Each potential swath connection (e.g., path between swathstart or end points) is assigned a cost based on a weighted average ofseveral subcosts. The subcosts may include time of travel along thepath, length of path, length of path not on a road,acceleration/deceleration requirements for travel along the path, amountof trim-back on swath, and so on. The swath connections may beformulated (block 310) as a traveling salesperson (TSP) problem whereeach where each “city” in the TSP is represented as an end-point of theswath. The purpose of the TSP is to connect the end-points of each swathin the lowest-cost ordering, per the costs listed above. Connections 186between the swath end-points are made by use of calls to thepoint-to-point planner system 172, which generates continuous curvaturepaths (clothoids) 188 between two points. Collision prevention betweenthe vehicles 10, 14 and/or the agricultural implements 12, 16 may beconducted via the choreographer system 168. For example, thechoreographer system 168 may prevent collisions at path intersections,when traveling in opposite directions on adjacent swaths, and/or whentraveling in the same direction over-taking one another.

The process 300 may then solve the TSP problem (block 312), for example,via a variety of TSP techniques such as Christofides' algorithm, nearestneighbor algorithm, bitonic tour, branch-and-bound algorithms, and soon. The resulting route between “cities” may then be provided (block314) as part of the vehicle plan(s) 180. The vehicles 10, 14 may receivethe plan(s) and may then operate based on the desired routes provided(block 314).

While only certain features of the disclosure have been illustrated anddescribed herein, many modifications and changes will occur to thoseskilled in the art. It is, therefore, to be understood that the appendedclaims are intended to cover all such modifications and changes as fallwithin the true spirit of the disclosure.

The invention claimed is:
 1. A path planning system for off-roadvehicles, comprising: a processor configured to: receive, via a planrequestor and executor system, a plan request; derive, via a swath andpartition generator system, a set of partitions and a set of swathscorresponding to the set of partitions based on the plan request;assign, via a swath connector, costs to potential connections betweenswaths in the set of swaths; derive a first vehicle plan based on thecosts to the potential connections between swaths, wherein the firstvehicle plan comprises a first path for a first off-road vehicle tooperate in a field, wherein the processor is configured to derive thefirst vehicle plan by formulating a traveling salesperson (TSP) problembased on the costs by deriving a plurality of cities, each citycorresponding to an end point of each of the swaths in the set ofswaths; and control the first off-road vehicle to operate in the fieldbased on the first vehicle plan.
 2. The path planning system of claim 1,wherein the processor is configured to formulate the TSP problem bytraveling through each city in a lowest-cost ordering based on thecosts.
 3. The path planning system of claim 1, wherein the costscomprise time of travel along the path, length of the path, length of aportion of the path not on a road, acceleration/decelerationrequirements for travel along the path, amount of trim-back on swath, ora combination thereof.
 4. The path planning system of claim 1, whereinthe first path comprises one or more clothoid segments.
 5. The pathplanning system of claim 1, wherein the path is C2 continuous.
 6. Thepath planning system of claim 1, wherein the processor is configured toderive a second vehicle plan based on the costs to the potentialconnections between swaths, wherein the second vehicle plan comprises asecond path for concurrent operations of the first off-road vehicle andthe second off-road vehicle in the field.
 7. The path planning system ofclaim 6, wherein the processor is configured to receive updated databased on current operations of the first off-road vehicle, the secondoff-road vehicle, or a combination thereof, and to derive an update tothe first vehicle plan, the second vehicle plan, or the combinationthereof, based on the current operations.
 8. A method of path planningfor off-road vehicles, comprising: receiving, via a plan requestor andexecutor system, a plan request; deriving, via a swath and partitiongenerator system, a set of partitions and a set of swaths correspondingto the set of partitions based on the plan request, wherein each swathcomprises a width at least as wide as an agricultural implement includedin a first off-road vehicle or being towed by the first off-road vehicleduring operations of the first off-road vehicle in the field; assigning,via a swath connector, costs to potential connections between swaths inthe set of swaths; deriving a first vehicle plan based on the costs tothe potential connections between swaths, wherein the first vehicle plancomprises a first path for the first off-road vehicle to operate in afield; and control the first off-road vehicle to operate in the fieldbased on the first vehicle plan.
 9. The method of claim 8, whereinderiving the first vehicle plan comprises formulating a travelingsalesperson (TSP) problem based on the costs.
 10. The method of claim 9,formulating the TSP problem comprises deriving a plurality of cities,each city corresponding to an end point of each of the swaths in the setof swaths.
 11. The method of claim 9, comprising deriving a secondvehicle plan based on the costs to the potential connections betweenswaths, wherein the second vehicle plan comprises a second path forconcurrent operations of the first off-road vehicle and the secondoff-road vehicle in the field.
 12. The method of claim 11, wherein thesecond path provides for a cost spacing suitable for increased cost asthe first and the second off-road vehicles come closer together.
 13. Atangible, non-transitory computer readable medium comprisinginstructions configured to: receive a plan request; derive a set ofpartitions and a set of swaths corresponding to the set of partitionsbased on the plan request; assign costs to potential connections betweenswaths in the set of swaths; derive a first vehicle plan based on thecosts to the potential connections between swaths, wherein the firstvehicle plan comprises a first route for a first off-road vehicle tooperate in a field; derive a second vehicle plan based on the costs tothe potential connections between swaths, wherein the second vehicleplan comprises a second route for concurrent operations of the firstoff-road vehicle and a second off-road vehicle in the field; and controlthe first off-road vehicle to operate in the field based on the firstvehicle plan and the second off-road vehicle to operate in the fieldbased on the second vehicle plan.
 14. The tangible, non-transitorycomputer readable medium comprising instructions of claim 13, whereinthe instructions configured to derive the first vehicle plan compriseinstructions configured to formulate a traveling salesperson (TSP)problem based on the costs.
 15. The tangible, non-transitory computerreadable medium comprising instructions of claim 14, wherein theinstructions are configured to formulate the TSP problem by deriving aplurality of cities, each city corresponding to an end point of each ofthe swaths in the set of swaths.
 16. The tangible, non-transitorycomputer readable medium comprising instructions of claim 13, whereinthe costs comprise time of travel along the first or second routes,length of the first or second routes, length of a portion of the firstor second routes not on a road, acceleration/deceleration requirementsfor travel along the first or second routes, amount of trim-back onswath, or a combination thereof.